%Filter design for Zeppelin project
%This filter is aplied in the elimination of noise of the
%accelerometer signal on controller.
%The ADC operates with 8 bit resolution and a sample period of 16ms.


clc
clear all

file=fopen('C:\Users\TONY\Desktop\ZP\Filtros\FiltroControlResultados.txt','wt');
fprintf(file,'//Filter design for Zeppelin project\n');
fprintf(file,'//This filter is aplied in the elimination of noise of the accelerometer signal on controller\n\n');

fs=1/(16e-3);

B=[1.000000000000000  -1.534044510876208   1.000000000000000];
A=[1.000000000000000  -1.820479940401335   0.835724077245025];

Gain=0.015234372848850*2.147503231709596;

fprintf(file,'//    fs = %+2.8e Hz\n',fs);

fprintf(file,'//    A = [ %+2.8e, %+2.8e, %+2.8e ]\n',A(1),A(2),A(3));
fprintf(file,'//    B = [ %+2.8e, %+2.8e, %+2.8e ]\n',B(1),B(2),B(3));
fprintf(file,'//    Gain = %+2.8e\n\n',Gain);

figure(4);
freqz(Gain*B,A,500,fs);

time=linspace(0,4,(fs+1)*4);
%signal = 0.5+0.35*sin(2*pi*2*time)+0.09*sin(2*pi*13*time)+0.09*sin(2*pi*31*time);
signal = 0.94+0.03*sin(2*pi*1*time)+0.015*sin(2*pi*13*time)+0.03*sin(2*pi*31*time);
u8signal=uint8(signal*255);

u16A=uint16(zeros(3,1)); %SQ14 format is used for represent the filter coeficients and gains
u16B=uint16(zeros(3,1)); 
u16Gain=uint16(Gain*2^18);	%Due to its small value, SQ18 into 16 bits is used to represent Gain value

if A(1)<0 u16A(1) = uint16(-A(1)*2^14); 	else u16A(1) = uint16(A(1)*2^14); end
if A(2)<0 u16A(2) = uint16(-A(2)*2^14); 	else u16A(2) = uint16(A(2)*2^14); end
if A(3)<0 u16A(3) = uint16(-A(3)*2^14); 	else u16A(3) = uint16(A(3)*2^14); end

if B(1)<0 u16B(1) = uint16(-B(1)*2^14); 	else u16B(1) = uint16(B(1)*2^14); end
if B(2)<0 u16B(2) = uint16(-B(2)*2^14); 	else u16B(2) = uint16(B(2)*2^14); end
if B(3)<0 u16B(3) = uint16(-B(3)*2^14); 	else u16B(3) = uint16(B(3)*2^14); end
	
fprintf(file,'const uint16 u16A[] = [ 0x%X, 0x%X, 0x%X ];   //SQ14\n',u16A(1),u16A(2),u16A(3));
fprintf(file,'const uint16 u16B[] = [ 0x%X, 0x%X, 0x%X ];   //SQ14\n',u16B(1),u16B(2),u16B(3));
fprintf(file,'const uint16 u16Gain = 0x%X;   //SQ18\n',u16Gain);

%Test Methodology
%Every variable will be tested for maximum value. It Maximum value is reached, variable will be printed on screen

overflow = 0;

%u8xr1 = uint16(128);    %Always positive value of the last input and output values
%u8xr2 = uint16(128);
%u16yr1 = uint16(3000);
%u16yr2 = uint16(3000);

u8xr1 = uint16(0);    %Always positive value of the last input and output values
u8xr2 = uint16(0);
u16yr1 = uint16(0);
u16yr2 = uint16(0);

u8signalfilt = zeros(1,length(u8signal),'uint8');     %This signal will contain the result in 8 bits after te gain multiplication
u16signalinter = zeros(1,length(u8signal),'uint16');   %This signal will contain the intermediate elements of the filtering in SQ8 format.
u32tempo=uint32(0);								      %Signal to save the intermedie value of mutiplications
    
for i=1:1:length(u8signal)
    
	% ***** B1 ******
    if( (double(u8signal(i)) * double(u16B(1))) > intmax('uint32'))	overflow=1;	disp(['Overflow en B1 con i = ',num2str(i)]); end
	u32tempo = uint32(u8signal(i)) * uint32(u16B(1));
    
	if( (double(u32tempo/(2^14)) > intmax('uint16')) ) overflow=1; disp(['Overflow en B1+ con i = ',num2str(i)]); end
	u16signalinter(i) = uint16(u32tempo/(2^14));
    
	% ***** B3 ******
	if( (double(u8xr2) * double(u16B(3))) > intmax('uint32'))	overflow=1;	disp(['Overflow en B3 con i = ',num2str(i)]); end
    u32tempo = uint32(u8xr2)*uint32(u16B(3));	
	
	if( (double(u16signalinter(i))+double(u32tempo/(2^14))) > intmax('uint16'))	overflow=1;	disp(['Overflow en B3+ con i = ',num2str(i)]); end
	u16signalinter(i) = u16signalinter(i) + uint16(u32tempo/(2^14));
	
	% ***** A2 *****
	if( (double(u16yr1) * double(u16A(2))) > intmax('uint32'))	overflow=1;	disp(['Overflow en A2 con i = ',num2str(i)]); end
	u32tempo = uint32(u16yr1)*uint32(u16A(2));
	
	if( (double(u16signalinter(i))+double(u32tempo/(2^14))) > intmax('uint16'))	overflow=1;	disp(['Overflow en A2+ con i = ',num2str(i)]); end
    u16signalinter(i) = u16signalinter(i) + uint16(u32tempo/(2^14));

	% ***** B2 ******
	if( (double(u8xr1) * double(u16B(2))) > intmax('uint32'))	overflow=1;	disp(['Overflow en B2 con i = ',num2str(i)]); end
    u32tempo = uint32(u8xr1)*uint32(u16B(2));
	
	if( (double(u16signalinter(i))-double(u32tempo/(2^14))) < intmin('uint16'))	overflow=1;	disp(['Overflow en B2+ con i = ',num2str(i)]); end
    u16signalinter(i) = u16signalinter(i) - uint16(u32tempo/(2^14));
    
 	% ***** A3 ******
	if( (double(u16yr2) * double(u16A(3))) > intmax('uint32'))	overflow=1;	disp(['Overflow en A3 con i = ',num2str(i)]); end
    u32tempo = uint32(u16yr2)*uint32(u16A(3));
	
	if( (double(u16signalinter(i))-double(u32tempo/(2^14))) < intmin('uint16'))	overflow=1;	disp(['Overflow en A3+ con i = ',num2str(i)]); end
    u16signalinter(i) = u16signalinter(i) - uint16(u32tempo/(2^14));
    
    u16yr2=u16yr1;
    u16yr1=u16signalinter(i);
    
    u8xr2=u8xr1;
    u8xr1=u8signal(i);
    
	if( (double(u16signalinter(i)) * double(u16Gain)) > intmax('uint32'))	overflow=1;	disp(['Overflow en Gain con i = ',num2str(i)]); end
    u32tempo = uint32(u16signalinter(i)) * uint32(u16Gain);
	
	if( (double(u32tempo/(2^18)) > intmax('uint8')))	overflow=1;	u8signalfilt(i) = uint8( (u32tempo/(2^18)) - 255 ); disp(['Overflow en Gain8 con i = ',num2str(i)]); 
	else
		u8signalfilt(i) = uint8(u32tempo/(2^18));
	end
end

figure(1);
plot(time,u8signalfilt);

figure(2);
plot(time,u8signal);

figure(3)
hold off
plot(time,u8signalfilt);
hold on
plot(time,u8signal);
plot(time,255*(0.94+0.03*sin(2*pi*1*time)))

if(overflow==1)
	fprintf(file,'//Overflow detected. Check overflow step and iteration')
end

fclose(file);
type FiltroControlResultados.txt